#include "protocol.h"
#include "utils/utils.h"
#include "iap.h"
#include "string.h"
#include "stdlib.h"


can_pack_t can_packs[CONFIG_CAN_PKG_COUNT];
can_frame_t can_rx_frames[CONFIG_CAN_TX_FRAMES];
can_frame_t can_tx_frames[CONFIG_CAN_RX_FRAMES];

static u8 can_tx_alloc_buff[NELEM(can_tx_frames) + 1];
byte_queue_t can_tx_allocator;

static u8 can_rx_alloc_buff[NELEM(can_rx_frames) + 1];
byte_queue_t can_rx_allocator;

static u8 can_tx_buff[NELEM(can_tx_frames) + 1];
byte_queue_t can_tx_queue = { can_tx_buff, NELEM(can_tx_buff), 0, 0 };

static u8 can_rx_buff[NELEM(can_rx_frames) + 1];
byte_queue_t can_rx_queue = { can_rx_buff, NELEM(can_rx_buff), 0, 0 };


// ================================================================================


bool can_poll_tx(can_frame_t *frame)
{
	return can_dev_tx_frame(frame);
}

__STATIC_INLINE can_pack_t *can_pack_get(u8 addr)
{
	can_pack_t *node, *node_end;
	can_pack_t *pack0, *pack1;

	pack1 = node = can_packs;
	pack0 = NULL;

	for (node_end = node + NELEM(can_packs); node < node_end; node++) {
		if (node->src == addr) {
			pack0 = node;
			continue;
		}

		if (node->times == 0xFFFF) {
			pack1 = node;
			continue;
		}

		if (node->times > pack1->times) {
			pack1 = node;
		}

		node->times++;
	}

	if (pack0 == NULL) {
		pack0 = pack1;
		pack0->src = addr;
		pack0->index = 0xFF;
	}

	pack0->times = 0;

	return pack0;
}

__STATIC_INLINE void can_pack_put(can_pack_t *pack)
{
	pack->times = 0xFFFF;
	pack->src = 0x00;
}

bool can_poll_rx(can_frame_t *frame)
{
	can_pack_t *pack = can_pack_get(frame->src);
	u8 length = frame->length + 1;
	u8 total = frame->total;
	u8 index = frame->index;

	if (index != pack->index) {
		if (index != 1) {
			return false;
		}

		pack->type = frame->type;
		pack->dest = frame->dest;
		pack->length = 0;
		pack->index = 2;
	} else {
		pack->index = (index + 1) & 0x1F;
	}

	memcpy(pack->data + pack->length, frame->data, length);
	pack->length += length;

	if (total == index) {
		iap_process_pack(pack);
		can_pack_put(pack);
	}

	return true;
}

void can_poll(void)
{
	u8 index;

	if (can_dev_get_error()) {
		can_dev_init();
	}

	if (byte_queue_read(&can_tx_queue, &index, 1)) {
		can_poll_tx(can_tx_frames + index);
		byte_queue_free(&can_tx_allocator, index);
	}

	if (byte_queue_read(&can_rx_queue, &index, 1)) {
		can_frame_t *frame = can_rx_frames + index;
		can_poll_rx(frame);
		byte_queue_free(&can_rx_allocator, index);
	}
}

bool can_send_frame(u32 efid, const void *buff)
{
	can_frame_t *frame;
	u8 index;

	index = byte_queue_alloc(&can_tx_allocator);
	if (index == 0xFF) {
		return false;
	}

	frame = can_tx_frames + index;
	frame->efid = efid;
	memcpy(frame->data, buff, frame->length + 1);

	if (byte_queue_write_byte(&can_tx_queue, index)) {
		return true;
	}

	byte_queue_free(&can_tx_allocator, index);

	return false;
}

static bool can_send_raw(u8 type, u8 dest, u8 src, const u8 *data, u16 length)
{
	u32 efid = type << 24 | (((length + 7) / 8) & 0x1F) << 19 | src << 7 | dest;
	u8 index = 1;
	u8 dlen = 8;

	while (length > 0) {
		if (length < 8) {
			dlen = length;
		}

		can_send_frame(efid | (dlen - 1) << 29 | index << 14, data);
		index = (index + 1) & 0x1F;
		length -= dlen;
		data += dlen;
	}

	return true;
}

bool can_send_command(u8 addr, const void *command, u16 length)
{
	return can_send_raw(1, addr, CONFIG_ADDR_ID, command, length);
}

bool can_send_response(u8 dest, u8 src, const void *response, u16 length)
{
	return can_send_raw(2, dest, src, response, length);
}

bool can_send_event(u8 addr, const void *event, u16 length)
{
	return can_send_raw(3, addr, CONFIG_ADDR_ID, event, length);
}

void can_hw_init(void)
{
	can_pack_t *pack, *pack_end;

	for (pack = can_packs, pack_end = pack + NELEM(can_packs); pack < pack_end; pack++) {
		pack->times = 0xFFFF;
	}

	byte_queue_alloc_init(&can_tx_allocator, can_tx_alloc_buff, sizeof(can_tx_alloc_buff));
	byte_queue_alloc_init(&can_rx_allocator, can_rx_alloc_buff, sizeof(can_rx_alloc_buff));

#ifdef CONFIG_CAN_SLP_PIO
	gpio2_output_write(CONFIG_CAN_SLP_PIO, CONFIG_CAN_SLP_VAL);
#endif

#ifdef CONFIG_CAN_PWR_PIO
	gpio2_output_write(CONFIG_CAN_SLP_PIO, CONFIG_CAN_PWR_VAL);
#endif
	can_dev_config();
}
